#include "stm32f10x.h"
#include <stdlib.h>
#include <math.h>
#include <stdio.h>
//位带操作,实现51类似的GPIO控制功能
//具体实现思想,参考<<CM3权威指南>>第五章(87页~92页).
//IO口操作宏定义
/***************************************************************************
*			舵机控制程序(下位机软件)
*	作者:ghimi
*	日期:2016.2.2
*	描述:配套舵机控制软件(上位机)来调节舵机动作
*	PA.9----USART_TX
*	PA.10---USART_RX
*	内蒙古大学鄂尔多斯学院
****************************************************************************/
#define BITBAND(addr, bitnum) ((addr & 0xF0000000)+0x2000000+((addr &0xFFFFF)<<5)+(bitnum<<2))

#define MEM_ADDR(addr)  *((volatile unsigned long  *)(addr))

#define BIT_ADDR(addr, bitnum)   MEM_ADDR(BITBAND(addr, bitnum))

//IO口地址映射
#define GPIOA_ODR_Addr    (GPIOA_BASE+12) //0x4001080C

#define GPIOB_ODR_Addr    (GPIOB_BASE+12) //0x40010C0C

#define GPIOC_ODR_Addr    (GPIOC_BASE+12) //0x4001100C

#define GPIOD_ODR_Addr    (GPIOD_BASE+12) //0x4001140C

#define GPIOE_ODR_Addr    (GPIOE_BASE+12) //0x4001180C

#define GPIOF_ODR_Addr    (GPIOF_BASE+12) //0x40011A0C

#define GPIOG_ODR_Addr    (GPIOG_BASE+12) //0x40011E0C


#define GPIOA_IDR_Addr    (GPIOA_BASE+8) //0x40010808

#define GPIOB_IDR_Addr    (GPIOB_BASE+8) //0x40010C08

#define GPIOC_IDR_Addr    (GPIOC_BASE+8) //0x40011008

#define GPIOD_IDR_Addr    (GPIOD_BASE+8) //0x40011408

#define GPIOE_IDR_Addr    (GPIOE_BASE+8) //0x40011808

#define GPIOF_IDR_Addr    (GPIOF_BASE+8) //0x40011A08

#define GPIOG_IDR_Addr    (GPIOG_BASE+8) //0x40011E08



//IO口操作,只对单一的IO口!
//确保n的值小于16!
#define PAout(n)   BIT_ADDR(GPIOA_ODR_Addr,n)  //输出

#define PAin(n)    BIT_ADDR(GPIOA_IDR_Addr,n)  //输入


#define PBout(n)   BIT_ADDR(GPIOB_ODR_Addr,n)  //输出

#define PBin(n)    BIT_ADDR(GPIOB_IDR_Addr,n)  //输入


#define PCout(n)   BIT_ADDR(GPIOC_ODR_Addr,n)  //输出

#define PCin(n)    BIT_ADDR(GPIOC_IDR_Addr,n)  //输入


#define PDout(n)   BIT_ADDR(GPIOD_ODR_Addr,n)  //输出

#define PDin(n)    BIT_ADDR(GPIOD_IDR_Addr,n)  //输入


#define PEout(n)   BIT_ADDR(GPIOE_ODR_Addr,n)  //输出

#define PEin(n)    BIT_ADDR(GPIOE_IDR_Addr,n)  //输入

#define PFout(n)   BIT_ADDR(GPIOF_ODR_Addr,n)  //输出

#define PFin(n)    BIT_ADDR(GPIOF_IDR_Addr,n)  //输入

#define PGout(n)   BIT_ADDR(GPIOG_ODR_Addr,n)  //输出

#define PGin(n)    BIT_ADDR(GPIOG_IDR_Addr,n)  //输入
#define	POUT0		PAout(0)
#define	POUT1		PAout(1)
#define	POUT2		PAout(2)
#define	POUT3		PAout(11)
#define	POUT4		PAout(12)
#define	POUT5		PAout(13)
#define	POUT6		PAout(14)
#define	POUT7		PAout(15)
#define	POUT8		PBout(0)
#define	POUT9		PBout(1)
#define	POUT10		PBout(2)
#define	POUT11		PBout(3)
#define	POUT12		PBout(4)
#define	POUT13		PBout(5)
#define	POUT14		PBout(6)
#define	POUT15		PBout(7)
#define	POUT16		PBout(8)
#define	POUT17		PBout(9)
#define	POUT18		PBout(10)
#define	POUT19		PBout(11)
#define	POUT20		PBout(12)
#define	POUT21		PBout(13)
#define	POUT22		PBout(14)
#define	POUT23		PBout(15)
/**************************************************************/
#define PIN(num) POUT##num
#define EN_USART1_RX     //使能串口接收
/**************************************************************/
unsigned char val8[24],val3[24];
//val3用于控制高电平脉冲的时间，val4用于配置val3与舵机角度的过渡
unsigned int dp,col;
char USART_RX_BUF[320];
_Bool USART_RX_STA;
_Bool	_20MS_FLAG=0;
struct{
	short int incre[24];	//单次插补的增量 ,用于替换val4
	short int  remain[24];	//增量的余数，用于替换dp0
	unsigned int remain_dp[24];	//余数的插补间隔
}Dp;//为插补数据创建结构体
/***************************************************************/
//用于计算插补增量
void Calc_Dp(unsigned char *ori,unsigned char *dest){
	unsigned char i;
	short int minus;
	for(i=0;i<24;i++){
		minus = dest[i] - ori[i];//插补的总增量
		Dp.incre[i] = minus/(int)dp;//单次插补增量
		Dp.remain[i] = minus%(int)dp;//单次插补增量余数
		Dp.remain_dp[i] = (dp/Dp.remain[i])?(dp/Dp.remain[i]):1;		//单次插补增量余数间隔,当结果为0时置为1
	}
}
//每隔20毫秒对PWM波进行更新
void UpdatePwm(void){
	unsigned char i;
	static unsigned int cnt = 0;;//用于统计插补次数

	if(cnt==0)Calc_Dp(val3,val8);//上一行动作执行完毕，开始执行下一行动作
	for (i=0;i<24;i++){
		val3[i]+=Dp.incre[i];//插补增量
		if(((cnt%Dp.remain_dp[i])==0)&&Dp.remain[i]){	//这里要注意Dp.remain_dp[i]不要等于0，当为0的时候舵机会变得不太稳定
			if(Dp.remain[i] > 0){
			  val3[i]++; //插补余数，增加方向
				Dp.remain[i]--;
			}
			else{
			  val3[i]--;//插补余数，减少方向
				Dp.remain[i]++;
			}
		}
	}
	if(cnt>=dp){			//本行到下一行的状态完成,开始到下下行

		cnt=0;
		col++;
	}else cnt++;		//对插补次数的计数，dp为插补的总数，当cnt==dp时，表示舵机已经从上一个动作到达下一个动作
}
void SystemInit(void);
unsigned char ASC_To_Valu(unsigned char asc);
void Handle_Order(char *ord);
#ifdef EN_USART1_RX //在定义了UART接收使能函数以后，通过重写fputc使能printf函数
	int fputc(int ch,FILE *f){
		USART_SendData(USART1,(char)ch);
		while(USART_GetFlagStatus(USART1,USART_FLAG_TC) == RESET);
		return ch;
	}
#endif
void USART_Config(void){
		USART_InitTypeDef USART_InitStructure;
		//USART 初始化设置
		USART_DeInit(USART1);  //复位串口1
		USART_InitStructure.USART_BaudRate =9600;//串口波特率 一般设置为9600;
		USART_InitStructure.USART_WordLength = USART_WordLength_8b;//字长为8位数据格式
		USART_InitStructure.USART_StopBits = USART_StopBits_1;//一个停止位
		USART_InitStructure.USART_Parity = USART_Parity_No;//无奇偶校验位
		USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None;//无硬件数据流控制
		USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;	//收发模式
		USART_Init(USART1, &USART_InitStructure); //初始化串口
		#ifdef EN_USART1_RX		  //如果使能了接收

		USART_ITConfig(USART1, USART_IT_RXNE, ENABLE);//开启中断
		#endif
		USART_Cmd(USART1, ENABLE);                    //使能串口
}

void RCC_Init(void)
{
		unsigned char HSEStartUpStatus;
		/*将外设RCC寄存器重设为缺省值*/
		RCC_DeInit();
		/*设置外部高速晶振（HSE）*/
		RCC_HSEConfig(RCC_HSE_ON);			//RCC_HSE_ON*****HSE晶振打开（ON）
		/*等待HSE起振*/
		HSEStartUpStatus=RCC_WaitForHSEStartUp();
		if(HSEStartUpStatus==SUCCESS)
		{
		/*设置AHB时钟（HCLK）*/
			RCC_HCLKConfig(RCC_SYSCLK_Div1);//RCC_SYSCLK_Div1--AHB	时钟=系统时钟
			/*设置高速AHB时钟（PCLK2）*/
			RCC_PCLK2Config(RCC_HCLK_Div1);//RCC_HCLK_Div1--APB2		时钟=HCLK
			/*设置低速AHB时钟（PCLK1）*/
			RCC_PCLK1Config(RCC_HCLK_Div2);//RCC_HCLK_Div2--APB1		时钟=HCLK/2
			/*设置PLL时钟源及倍频系数*/
			RCC_PLLConfig(RCC_PLLSource_HSE_Div1,RCC_PLLMul_9);//PLL的输入时钟=HSE时钟频率；RCC_PLLMul_9--PLL输入时钟x9
			/*使能PLL*/
			RCC_PLLCmd(ENABLE);
			/*检查指定的RCC标志位（PLL准备好标志）设置与否*/
			while(RCC_GetFlagStatus(RCC_FLAG_PLLRDY)==RESET);
			/*设置系统时钟（SYSCLK）*/
			FLASH_SetLatency(FLASH_Latency_2);//设置FLASH代码延时
			FLASH_PrefetchBufferCmd(FLASH_PrefetchBuffer_Enable);//使能预取指缓存
			/*设置系统时钟（SYSCLK）*/
			RCC_SYSCLKConfig(RCC_SYSCLKSource_PLLCLK);//RCC_SYSCLKSource_PLLCLK--选择PLL作为系统时钟
			/*PLL返回用作系统时钟的时钟源*/
			while(RCC_GetSYSCLKSource()!=0x08);//0x08:PLL作为系统时钟
			RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2|RCC_APB1Periph_TIM3|RCC_APB1Periph_TIM4, ENABLE); //时钟使能
			RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1|RCC_APB2Periph_GPIOA|RCC_APB2Periph_GPIOB|RCC_APB2Periph_GPIOC,ENABLE); //时钟使能
		}
}
/*********************************************************************
* Function Name  : Set_Polar
* Description    : 设置引脚电平高低。
* Input          : unsigned char num：舵机编号，_Bool pol 引脚极性
* Output         : None
* Return         : None
**********************************************************************/
void Set_Polar(unsigned char num,_Bool pol){
	switch(num){
		case 0:PIN(0) = pol;break;
		case 1:PIN(1) = pol;break;
		case 2:PIN(2) = pol;break;
		case 3:PIN(3) = pol;break;
		case 4:PIN(4) = pol;break;
		case 5:PIN(5) = pol;break;
		case 6:PIN(6) = pol;break;
		case 7:PIN(7) = pol;break;
		case 8:PIN(8) = pol;break;
		case 9:PIN(9) = pol;break;
		case 10:PIN(10) = pol;break;
		case 11:PIN(11) = pol;break;
		case 12:PIN(12) = pol;break;
		case 13:PIN(13) = pol;break;
		case 14:PIN(14) = pol;break;
		case 15:PIN(15) = pol;break;
		case 16:PIN(16) = pol;break;
		case 17:PIN(17) = pol;break;
		case 18:PIN(18) = pol;break;
		case 19:PIN(19) = pol;break;
		case 20:PIN(20) = pol;break;
		case 21:PIN(21) = pol;break;
		case 22:PIN(22) = pol;break;
		case 23:PIN(23) = pol;break;
		default:break;
	}
	return;
}
/*******************************************************************************
* Function Name  : TIM2_IRQHandler
* Description    : This function handles TIM2 global interrupt request.
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void TIM2_IRQHandler(void)
{
	static unsigned int val1=0;
	unsigned char num;//变量区间 范围为0~7
	if (TIM_GetITStatus(TIM2, TIM_IT_Update) != RESET)  //检查TIM3更新中断发生与否
	TIM_ClearITPendingBit(TIM2, TIM_IT_Update  );  //清除TIMx更新中断标志

	//num = (val1/250)    得到结果为0~7,这里控制第0号到第7号舵机
	num = val1/250;
	Set_Polar(num,(val1%250)/val3[num]);
	val1 ++;
	if(val1==2000){
		val1=0;
		_20MS_FLAG=1;		//20ms更新标志位
	}
}
/*******************************************************************************
* Function Name  : TIM3_IRQHandler
* Description    : This function handles TIM3 global interrupt request.
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void TIM3_IRQHandler(void)
{
	static unsigned int val1=0;
	unsigned char num;
	if (TIM_GetITStatus(TIM3, TIM_IT_Update) != RESET)  //检查TIM3更新中断发生与否
	TIM_ClearITPendingBit(TIM3, TIM_IT_Update  );  //清除TIMx更新中断标志

	num = (val1/250)+8;		//控制9到15号舵机
	Set_Polar(num,(val1%250)/val3[num]); //设置引脚电平高低
	val1 ++;
	if(val1>=2000)val1=0;	//当达到20毫秒计数时，重新开始计数
}
/*******************************************************************************
* Function Name  : TIM4_IRQHandler
* Description    : This function handles TIM4 global interrupt request.
* Input          : None
* Output         : None
* Return         : None
*******************************************************************************/
void TIM4_IRQHandler(void)
{
	static unsigned int val1=0;
	unsigned char num;
	if (TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET)  //检查TIM3更新中断发生与否
	TIM_ClearITPendingBit(TIM4, TIM_IT_Update  );  //清除TIMx更新中断标志
	
	num = (val1/250)+16;//控制16到23号舵机
	Set_Polar(num,(val1%250)/val3[num]);  //设置引脚电平高低
	val1 ++;
	if(val1>=2000)val1=0;	//当达到20毫秒计数时，重新开始计数
}
void TIM_Init(void)
{
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;//定时器TIM3初始化
	TIM_TimeBaseStructure.TIM_Period = 5; //设置在下一个更新事件装入活动的自动重装载寄存器周期的值

	TIM_TimeBaseStructure.TIM_Prescaler =119; //设置用来作为TIMx时钟频率除数的预分频值
	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; //设置时钟分割:TDTS = Tck_tim
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;  //TIM向上计数模式
	TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); //根据指定的参数初始化TIMx的时间基数单位
	TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE ); //使能指定的TIM3中断,允许更新中断
	TIM_Cmd(TIM2, ENABLE);  //使能TIMx

	TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure); //根据指定的参数初始化TIMx的时间基数单位
	TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE); //使能指定的TIM3中断,允许更新中断
	TIM_Cmd(TIM3, ENABLE);  //使能TIMx

	TIM_TimeBaseInit(TIM4, &TIM_TimeBaseStructure); //根据指定的参数初始化TIMx的时间基数单位
	TIM_ITConfig(TIM4,TIM_IT_Update,ENABLE ); //使能指定的TIM3中断,允许更新中断
	TIM_Cmd(TIM4, ENABLE);  //使能TIMx

}
void GPIO_Config(void)
{
	/* Private typedef -----------------------------------------------------------*/
	GPIO_InitTypeDef  GPIO_InitStructure;             //Added 08-18-2009

	/* Enable GPIOA clock                                                 */
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);		 // Added 08-18-2009
	/* Configure PA_All as outputs push-pull, max speed 50 MHz               */
	GPIO_InitStructure.GPIO_Pin =  GPIO_Pin_All;				 // Added 08-18-2009
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;			 // Added 08-18-2009
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;			 // Added 08-18-2009
	GPIO_Init(GPIOA, &GPIO_InitStructure);					 // Added 08-18-2009
	GPIO_Init(GPIOB, &GPIO_InitStructure);					 // Added 08-18-2009
	GPIO_Init(GPIOC, &GPIO_InitStructure);					 // Added 08-18-2009



	//USART1_TX			PA.9
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_9; //PA.9----USART-TX
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP;	//复用推挽输出
	GPIO_Init(GPIOA, &GPIO_InitStructure); //初始化PA9

	//USART1_RX	  PA.10
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_10;//PA.10----USART-RX
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;//浮空输入
	GPIO_Init(GPIOA, &GPIO_InitStructure);  //初始化PA10
}
void NVIC_Config(void)
{
	NVIC_InitTypeDef NVIC_InitStructure;
	//中断优先级NVIC设置
		/* #ifdef...#else...#endid结构的作用是根据预编译条件决定中断向量表起始地址*/
	#ifdef VECT_TAB_RAM
	/*中断向量表起始地址从0x20000000开始*/
	NVIC_SetVectorTable(NVIC_VectTab_RAM,0x0);
	#else /*VECT_TAB_FLASH*/
	/*中断向量表起始地址从0x8000000开始*/
	NVIC_SetVectorTable(NVIC_VectTab_FLASH,0x0);
	#endif
	/*选择NVIC优先级分组2*/
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
	//中断优先级NVIC设置
	NVIC_InitStructure.NVIC_IRQChannel = TIM3_IRQn;  //TIM3中断
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;  //先占优先级0级
	NVIC_InitStructure.NVIC_IRQChannelSubPriority =3;  //从优先级3级
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道被使能
	NVIC_Init(&NVIC_InitStructure);  //初始化NVIC寄存器
	//中断优先级NVIC设置
	NVIC_InitStructure.NVIC_IRQChannel = TIM4_IRQn;  //TIM4中断
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;  //先占优先级0级
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;  //从优先级2级
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道被使能
	NVIC_Init(&NVIC_InitStructure);  //初始化NVIC寄存器
	//中断优先级NVIC设置
	NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;  //TIM2中断
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;  //先占优先级0级
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;  //从优先级2级
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; //IRQ通道被使能
	NVIC_Init(&NVIC_InitStructure);  //初始化NVIC寄存器
	//中断优先级NVIC配置
	NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=1 ;//抢占优先级3
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;		//子优先级3
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;			//IRQ通道使能
	NVIC_Init(&NVIC_InitStructure);	//根据指定的参数初始化NVIC寄存器

}

int main(void)
{
	while(1)
	{
		if(USART_RX_STA)//当 接收到数据标志位置位时,处理接收数据
		{
			Handle_Order(USART_RX_BUF);             //处理接收到的数据
			USART_RX_STA=0;
		}
		if(_20MS_FLAG)//当20毫秒标志位置位时,更新PWM
		{
			UpdatePwm();
			_20MS_FLAG=0;
		}
	}
}
void SystemInit(void)
{
	RCC_Init();//系统时钟初始化,使用PLL作为系统时钟
	TIM_Init();//定时器初始化
	GPIO_Config();//GPIO配置
	USART_Config();//串口通讯配置
	NVIC_Config();//中断初始化,包含定时器中断和串口中断
}
void USART1_IRQHandler(void)                	//串口1中断服务程序
{
	static unsigned short int val5=0;

	if(USART_GetITStatus(USART1, USART_IT_RXNE) != RESET)  //接收中断
	{
		USART_RX_BUF[val5]=USART_ReceiveData(USART1);//(USART1->DR);	//读取接收到的数据
		if(USART_RX_BUF[val5] == 0x0a){ 			//当接收到一行结束的信号时，使能处理数据
			val5 = 0;
		  USART_RX_STA = ENABLE;
		}else val5++;	   //当没有接收到一行的结束信号时，继续读取该行
		if(val5>=320 || !((USART_RX_BUF[0] == ' ') || (USART_RX_BUF[0] == '#')) )val5=0;//接收数据异常，初始化数组
	}
}
void Handle_Order(char *ord)
{
unsigned int val6,val7;//val6用于标识引脚号(舵机号)，val7用于标识脉冲值
	if(ord[0]=='#')
	{
		//接收一个舵机动作至val3[]
			if(ord[4]=='p')
			{
				val6=ASC_To_Valu(ord[1])*10+ASC_To_Valu(ord[2]);//读取指令中的引脚号(舵机号)
				if(ASC_To_Valu(ord[6])>=5)val7=ASC_To_Valu(ord[6])*10+ASC_To_Valu(ord[7]);
				else if(ASC_To_Valu(ord[6])<=2)val7=ASC_To_Valu(ord[6])*100+
										ASC_To_Valu(ord[7])*10+ASC_To_Valu(ord[8]);//读取当前的舵机角度
			}
			else if(ord[3]=='p')
			{
				val6=ASC_To_Valu(ord[1]);//读取指令中的引脚号(舵机号)
				if(ASC_To_Valu(ord[5])>=5)val7=ASC_To_Valu(ord[5])*10+ASC_To_Valu(ord[6]);
				else if(ASC_To_Valu(ord[5])<=2)val7=ASC_To_Valu(ord[5])*100+
										ASC_To_Valu(ord[6])*10+ASC_To_Valu(ord[7]);//读取当前的舵机角度
			}
			if(val7>=250)val7=249;
			//val6为舵机号，val7为舵机的正周期数0-250
			if(val6<24)	val8[val6] = val3[val6]=val7;//执行指令,只处理0--23号舵机的值
		}
	else if(ord[0]==' ')
	{
    //接收一组舵机动作至val8[]
		ord++;
		val6=25;
		while(1)
		{
					if(ord[0]=='#'&&ord[4]=='P')
					{
						val6=ASC_To_Valu(ord[1])*10+ASC_To_Valu(ord[2]);//读取指令中的引脚号(舵机号)
						if(ASC_To_Valu(ord[5])>=5)
						{
							val7=ASC_To_Valu(ord[5])*10+ASC_To_Valu(ord[6]);
							ord+=8;
						}
						else if(ASC_To_Valu(ord[5])<=2)
						{
							val7=ASC_To_Valu(ord[5])*100+
										ASC_To_Valu(ord[6])*10+ASC_To_Valu(ord[7]);//读取当前的舵机角度
							ord+=8;
						}
					}
					else if(ord[0]=='#'&&ord[3]=='P')
					{
						val6=ASC_To_Valu(ord[1]);//读取指令中的引脚号(舵机号)
						if(ASC_To_Valu(ord[4])>=5)
						{
							val7=ASC_To_Valu(ord[4])*10+ASC_To_Valu(ord[5]);
							ord+=8;
						}
						else if(ASC_To_Valu(ord[4])<=2)
						{

							val7=ASC_To_Valu(ord[4])*100+
										ASC_To_Valu(ord[5])*10+ASC_To_Valu(ord[6]);//读取当前的舵机角度
							ord+=8;
						}
					}
					else if(ord[0]=='T')
					{
						dp=atoi(ord+1)/20;
						while(*ord)
						{
							*ord='\0';
							ord++;
						}
						break;
					}
					else ord++;
			if(val7>=250)val7=249;
			if(val6<24)	val8[val6]=val7;//只处理0--23号舵机的值
		}
	}
}
/**********************************************************************
文件名称：ASC_VALU.C
文件功能：将字符型转化成数值型
***********************************************************************/
unsigned char ASC_To_Valu(unsigned char asc)
{
	unsigned char valu;
	switch(asc)
	{
		case 0x30:valu=0;break;	  //0的ASC码时0x30,,十进制48
		case 0x31:valu=1;break;
		case 0x32:valu=2;break;
		case 0x33:valu=3;break;
		case 0x34:valu=4;break;
		case 0x35:valu=5;break;
		case 0x36:valu=6;break;
		case 0x37:valu=7;break;
		case 0x38:valu=8;break;
		case 0x39:valu=9;break;
	}
		return valu;
}
unsigned char str2val(char *str){
	unsigned char temp;
	while(*str != 0){
		if((*str < '0') ||(*str > '9'))break;//如果当前字符不是数字字符，则跳出循环
		else {
			temp = temp*10 + (*str-'0');//如果当前字符时数字字符则计算数值
			str++;//移到下一个字符
		}		
	}
	return temp;
}
unsigned short int str2val2(char *str){
	unsigned short int temp;
	while(*str != 0){
		if((*str >= '0') ||(*str <= '9')){		//如果当前字符不是数字字符，则跳出循环
			if(*(str+1) >= '0' || (*(str+1) <= '9'))temp = temp*10 + (*str-'0');//如果当前字符时数字字符则计算数值
			else break;//如果下一个字符不是数字字符则跳出循环避开乘个位数
			str++;//移到下一个字符
		}else break;
	}
	return temp;
}
